/*
 * KKMathUtils.cpp
 *
 *  Created on: 2013-9-15
 *      Author: wanghua
 */

#include "KKMathUtils.h"

static inline float vectorDot(const float* a, const float* b, int m) {
	float r = 0;
	while (m--) {
		r += *(a++) * *(b++);
	}
	return r;
}

static inline float vectorNorm(const float* a, int m) {
	float r = 0;
	while (m--) {
		float t = *(a++);
		r += t * t;
	}
	return sqrtf(r);
}

#define DEFAULT_DEGREE 2
#define MAX_POINT_SIZE 5

bool solveLeastSquares(const float* x, const float* y, int m, int n,
		float* outB, float* outDet) {

	// Expand the X vector to a matrix A.
	float a[DEFAULT_DEGREE][MAX_POINT_SIZE]; // column-major order
	for (int h = 0; h < m; h++) {
		a[0][h] = 1;
		for (int i = 1; i < n; i++) {
			a[i][h] = a[i - 1][h] * x[h];
		}
	}

	// Apply the Gram-Schmidt process to A to obtain its QR decomposition.
	float q[DEFAULT_DEGREE][MAX_POINT_SIZE]; // orthonormal basis, column-major order
	float r[MAX_POINT_SIZE][MAX_POINT_SIZE]; // upper triangular matrix, row-major order
	for (int j = 0; j < n; j++) {
		for (int h = 0; h < m; h++) {
			q[j][h] = a[j][h];
		}
		for (int i = 0; i < j; i++) {
			float dot = vectorDot(&q[j][0], &q[i][0], m);
			for (int h = 0; h < m; h++) {
				q[j][h] -= dot * q[i][h];
			}
		}

		float norm = vectorNorm(&q[j][0], m);
		if (norm < 0.000001f) {
			// vectors are linearly dependent or zero so no solution
			return false;
		}

		float invNorm = 1.0f / norm;
		for (int h = 0; h < m; h++) {
			q[j][h] *= invNorm;
		}
		for (int i = 0; i < n; i++) {
			r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m);
		}
	}

	// Solve R B = Qt Y to find B.  This is easy because R is upper triangular.
	// We just work from bottom-right to top-left calculating B's coefficients.
	for (int i = n; i-- != 0;) {
		outB[i] = vectorDot(&q[i][0], y, m);
		for (int j = n - 1; j > i; j--) {
			outB[i] -= r[i][j] * outB[j];
		}
		outB[i] /= r[i][i];
	}

	// Calculate the coefficient of determination as 1 - (SSerr / SStot) where
	// SSerr is the residual sum of squares (squared variance of the error),
	// and SStot is the total sum of squares (squared variance of the data).
	float ymean = 0;
	for (int h = 0; h < m; h++) {
		ymean += y[h];
	}
	ymean /= m;

	float sserr = 0;
	float sstot = 0;
	for (int h = 0; h < m; h++) {
		float err = y[h] - outB[0];
		float term = 1;
		for (int i = 1; i < n; i++) {
			term *= x[h];
			err -= term * outB[i];
		}
		sserr += err * err;
		float var = y[h] - ymean;
		sstot += var * var;
	}
	*outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1;
	return true;
}

bool isSegmentIntersect(const KKPoint& start1, const KKPoint& end1,
		const KKPoint& start2, const KKPoint& end2) {

	return ccpSegmentIntersect(ccp(start1.x, start1.y), ccp(end1.x, end1.y),
	ccp(start2.x, start2.y), ccp(end2.x, end2.y));

}

#define FAR_POINT_X 100000.f

bool isPointInPoly(KKVertexArray& poly, const KKPoint& point,
		bool includeEdge) {

	const unsigned int& vertexCount = poly.count();

	assert(vertexCount > 2);

	int intersectCount = 0;

	KKPoint farPoint(FAR_POINT_X, point.y);

	for (unsigned int i = 0; i < vertexCount; i++) {
		const KKPoint & vertexA = *(poly.getObjectAtIndex(i));
		const KKPoint & vertexB =
				*(poly.getObjectAtIndex((i + 1) % vertexCount));
		if (isSegmentIntersect(vertexA, vertexB, point, farPoint)) {
			intersectCount++;
		}
	}

	return intersectCount % 2 == 1;
}

KKMathUtils::KKMathUtils() {
	// TODO Auto-generated constructor stub

}

KKMathUtils::~KKMathUtils() {
	// TODO Auto-generated destructor stub
}
